#include "DeviceInertialNavigation.h"
#include "common.h"

using namespace std;

DeviceInertialNavigation::~DeviceInertialNavigation() {
    close(m_fd);
    m_fd = -1;
}

void DeviceInertialNavigation::func(Aipov_Data& data) {
    getBuf();

    // cout << "Inertial data:" << m_buf << endl;

    // 解析AIPOV格式数据
    // unsigned long hex1, hex2;
    sscanf(m_buf.c_str(), "$AIPOV,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,0x%lx*%lx",
        &data.utc_time, &data.heading, &data.roll, &data.pitch,
        &data.angular_velocity_x, &data.angular_velocity_y, &data.angular_velocity_z,
        &data.acceleration_x, &data.acceleration_y, &data.acceleration_z,
        &data.latitude, &data.longitude, &data.altitude,
        &data.velocity_east, &data.velocity_north, &data.velocity_up,
        &data.non, &data.non, &data.non, &data.non,
        &data.hex1, &data.hex2
    );

    data.velocity = sqrt(
            data.velocity_east*data.velocity_east+
            data.velocity_north*data.velocity_north+
            data.velocity_up*data.velocity_up
        );
}

void DeviceInertialNavigation::sendMessage(string message){
    write(m_fd, message.c_str(), message.size());

    return;
}